#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/transform_listener.hpp>
#include <tf2_ros/create_timer_ros.hpp>
#include <message_filters/subscriber.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <tf2_ros/message_filter.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

using namespace std::chrono_literals;
class MsgFilterCppNode : public rclcpp::Node
{
private:
    std::unique_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    std::shared_ptr<tf2_ros::CreateTimerROS> timer_;
    message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub;
    std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> filter;

public:
    MsgFilterCppNode() : Node("msg_filter_cpp_node")
    {
        RCLCPP_INFO(this->get_logger(), "MsgFilterCppNode创建");
        timer_ = std::make_shared<tf2_ros::CreateTimerROS>(this->get_node_base_interface(),
                                                           this->get_node_timers_interface());
        buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        buffer_->setCreateTimerInterface(timer_);
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
        point_sub.subscribe(this, "point");
        // geometry_msgs::msg::PointStamped 过滤的消息类型
        /*
        BufferT & buffer, const std::string & target_frame, uint32_t queue_size,
        const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging,
        const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock,
        std::chrono::duration<TimeRepT, TimeT> buffer_timeout =
        std::chrono::duration<TimeRepT, TimeT>::max())

        */
        filter = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
            point_sub,
            *buffer_,
            "base_link",
            10,
            this->get_node_logging_interface(),
            this->get_node_clock_interface(),
            1s);
        //
        filter->registerCallback(&MsgFilterCppNode::trans_point, this);
    }

private:
    void trans_point(const geometry_msgs::msg::PointStamped &ps)
    {
        // void trans_point(const geometry_msgs::msg::PointStamped::ConstSharedPtr ps)
        //  geometry_msgs::msg::PointStamped ps_t = buffer_->transform(ps, "base_link");
        auto ps_t = buffer_->transform(ps, "base_link");
        RCLCPP_INFO(this->get_logger(), "父坐标系%s 坐标(%.2f,%.2f,%.2f)",
                    ps_t.header.frame_id.c_str(),
                    ps_t.point.x,
                    ps_t.point.y,
                    ps_t.point.z);
    }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MsgFilterCppNode>());
    rclcpp::shutdown();
    return 0;
}
/*
坐标点变换
1,雷达到base_link 和laser 的相对姿态
2，发布point 到laser 的坐标
2，求解point 到base_link 的相对坐标
run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 0.4 --z 0.2
ros2 run  cpp03_tf_broadcaster demo03_point_tf_broadcast
ros2 run cpp04_tf_listener demo02_msg_filter
*/
/*
point_sub 只是单纯订阅 point 话题；

listener_ 保持 TF 的最新坐标系关系；

filter 负责把二者对接起来，保证回调只在 TF 可用时触发；

回调里用 buffer_->transform，就能拿到点在目标坐标系（base_link）下的坐标。
filter 并不会自动做坐标变换，它只是保证“这个点的 TF 是可用的”再触发回调。
真正的坐标变换还是要你在回调里自己调用：

auto ps_t = buffer_->transform(*ps, "base_link");
你可以把 MessageFilter 看成一个 同步器/门控器：只有当 消息 + TF 都 ready，它才会放行，调用你注册的回调
*/